/*
 * Vector2D.java
 *
 * Created on 2 de agosto de 2007, 06:00 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package nx.utils;

import javaclient3.structures.*;
/**
 *
 * @author jonathan
 */
public class Vector2D extends PlayerPose{
    
    public static final int MODE_XY=0;
    public static final int MODE_MA=1;
    
    //private double x;
    //private double y;
    private double angle;
    private double magnitude;
    
    /**
     * Creates a new instance of Vector2D
     */
    public Vector2D()
    {
        super();
        magnitude = 0;
        angle = 0;
                
    
    }
    public Vector2D(Vector2D vec)
    {
        super(vec.getPx(),vec.getPy(),vec.getPa());
        this.magnitude = vec.getMag();
        this.angle = vec.getAngle();

    }

    public Vector2D(PlayerPose pp)
    {
        this.setPx(pp.getPx());
        this.setPy(pp.getPy());

        angle = (float)Math.atan2((float)this.getPy(),(float)this.getPx());
        magnitude = (float)Math.sqrt((double)(this.getPx()*this.getPx()+this.getPy()*this.getPy()));

    }

    public Vector2D(double val1, double val2, int mode)
    {
        if(mode==MODE_XY){

            this.setPx(val1);
            this.setPy(val2);

            
            angle = (float)Math.atan2((float)this.getPy(),(float)this.getPx());
            magnitude = (float)Math.sqrt((double)(this.getPx()*this.getPx()+this.getPy()*this.getPy()));
            
        }
        else{

            magnitude = val1;
            angle = val2;
            
            this.setPx(magnitude*(float)Math.cos((double) angle));
            this.setPy(magnitude*(float)Math.sin((double) angle));
            
        }
    
    }
    
    public void print(int mode)
    {
        if(mode == this.MODE_XY)
            System.out.println("VECTOR2D: (x, y): ("+this.getPx()+", "+this.getPy()+")\n");
        else
            System.out.println("VECTOR2D: (mag, theta): ("+this.getMag()+", "+ Math.toDegrees(this.getAngle())+")");
        
    }
    
    public void setX(float x){
        this.setPx(x);
        angle = (float)Math.atan2((float)this.getPy(),(float)this.getPx());
        magnitude = (float)Math.sqrt((double)(this.getPx()*this.getPx()+this.getPy()*this.getPy()));
    }
    
    public void setY(float y){
        this.setPy(y);
        angle = (float)Math.atan2((float)this.getPy(),(float)this.getPx());
        magnitude = (float)Math.sqrt((double)(this.getPx()*this.getPx()+this.getPy()*this.getPy()));
    }
    
    public void setAngle(float theta){
        this.angle=theta;
        if(magnitude!=0){
            this.setPx(magnitude*(float)Math.cos((double) theta));
            this.setPy(magnitude*(float)Math.sin((double) theta));
        }
        else
        {
            this.setPx(0);
            this.setPy(0);
        }
    }
    //FIXME: Revisar cuuando se pasa una magnitude negativa!!!
    public void setMag(float mag){
        magnitude=mag;
        if(magnitude!=0){
            this.setPx(magnitude*(float)Math.cos((double) angle));
            this.setPy(magnitude*(float)Math.sin((double) angle));
        }
        else
        {
            this.setPx(0);
            this.setPy(0);
        }
    }
    
    public double getX(){ return this.getPx();}
    public double getY(){ return this.getPy();}
    public double getAngle(){ return angle;}
    public double getMag(){ return magnitude;}
    
    public Vector2D add(Vector2D vector){
        return new Vector2D(this.getPx()+vector.getX(), this.getPy()+vector.getY(),MODE_XY);
    }
    
    /**
     * Sub
     * @param vector escala a aplicar
     * @return Vector new vector after scale
     */
    public Vector2D sub(Vector2D vector){
        return new Vector2D(this.getPx()-vector.getX(), this.getPy()-vector.getY(), MODE_XY);
    }
    
    /**
     * This method .. scale the vector
     * @param escala escala a aplicar
     * @return Vector new vector after scale
     */
    public Vector2D scale(double escala){
        return new Vector2D(escala*this.getPx(),escala*this.getPy(),MODE_XY);
    }
    
    /**
     * Normaliza el vector.
     * @return Vector new vector after normalize
     */
    public Vector2D normalize(){
        if(magnitude!=0)
            return new Vector2D(this.getPx()/this.magnitude,this.getPy()/this.magnitude,MODE_XY);
        else
            return new Vector2D(0,0,MODE_XY);
    }
    
    /**
     * Rota el plano de referencia del robot a angle en radianes.
     * @param angle angulo al que se rotara en radianes
     * @return Vector new vector after rotation
     */
    public Vector2D rotate(double angle){
        return new Vector2D(this.magnitude,(this.angle+angle),MODE_MA);
    }

    //correct angle in order to be in the range [0, 2*pi]
    public static double angleCorrZero22PI(double angle)
    {
        double newAngle = angleCorrMPI2PI(angle);

        if(newAngle<0)
            newAngle += 2*Math.PI;

        return newAngle;


    }
    //correct angle in order to be in the range [-pi, pi]
    public static double angleCorrMPI2PI(double angle)
    {
        int laps = 0;
        
        //correct laps if angle is higher than one lap (2 PI)
        if(Math.abs(angle)>2*Math.PI)
        {
            //eliminate laps from angle
            laps = (int)(angle / (2*Math.PI));
            angle -= laps*2*Math.PI;
            
        }
        //System.out.println("Angle after laps: "+angle);
        //then correct angle in order to have a result between [-PI, PI]
        if(Math.abs(angle)>Math.PI)
        {
            if(angle<0)
                angle += 2*Math.PI;
            else
                angle -= 2*Math.PI;
        }
        return angle;
    
    }

    public static boolean angleGreaterThan(double angle1, double angle2)
    {
        double a1 = angleCorrMPI2PI(angle1);
        double a2 = angleCorrMPI2PI(angle2);

        if(a1<0)
            a1 += 2*Math.PI;
        if(a2<0)
            a2 += 2*Math.PI;

        System.out.println("a1: "+a1+" a2: "+a2);
        return a2>a1;

    }
    public double distanceTo(Vector2D point){
        return Math.sqrt(Math.pow((this.getPx()-point.getX()),2.0f)+Math.pow((this.getPy()-point.getY()),2.0f));
    }
    
    static public Vector2D averageVector(Vector2D [] vectors){
        float sumX=0;
        float sumY=0;
        for(int i=0;i<vectors.length;i++){
            sumX+=vectors[i].getX();
            sumY+=vectors[i].getY();
        }
        sumX/=vectors.length;
        sumY/=vectors.length;
        
        return new Vector2D(sumX,sumY,MODE_XY);
    }
    
    public String toString(){
        return new String("X= "+this.getPx()+"\tY= "+this.getPy()+"\n");
    }

}
